#include <ros/ros.h>
#include <mavros_msgs/State.h>
#include <sensor_msgs/NavSatFix.h>

mavros_msgs::State current_state;
sensor_msgs::NavSatFix current_position;

void stateCallback(const mavros_msgs::State::ConstPtr& msg) {
    current_state = *msg;
}

void positionCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
    current_position = *msg;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "gps_listener");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, stateCallback);
    ros::Subscriber position_sub = nh.subscribe<sensor_msgs::NavSatFix>("mavros/global_position/global", 10, positionCallback);

    ros::Rate rate(10.0);
    while (ros::ok()) {
        ros::spinOnce();
        
        if (current_state.connected) {
            ROS_INFO("Current GPS Position: Lat: %f, Lon: %f, Alt: %f",
                     current_position.latitude,
                     current_position.longitude,
                     current_position.altitude);
        } else {
            ROS_WARN("Drone not connected.");
        }

        rate.sleep();
    }

    return 0;
}
